#include "TubeIdle.h"
#include "../Robotmap.h"
#include "WPILib.h"

TubeIdle::TubeIdle() {
	// Use requires() here to declare subsystem dependencies
	Requires(tube);
}

// Called just before this Command runs the first time
void TubeIdle::Initialize() {
}

// Called repeatedly when this Command is scheduled to run
void TubeIdle::Execute() {
	
	tube->doNothing();
	
	bool b1, b2, b3;
	
	b1 = tube->getBallSensorStatus(RobotMap::FEED_BALL_1_SENS);
	b2 = tube->getBallSensorStatus(RobotMap::FEED_BALL_2_SENS);
	b3 = tube->getBallSensorStatus(RobotMap::FEED_BALL_3_SENS);
	
	if(b1 && b2 && b3 && oi->isPickingBalls())
	{
		tube->setBrushOff();
		ballFeeder->setBrushOff();
	}
	
	oi->setBallOneStatus(b1);
	oi->setBallTwoStatus(b2);
	oi->setBallThreeStatus(b3);
	
	SmartDashboard::Log(b1, "Ball 1 Status");
	SmartDashboard::Log(b2, "Ball 2 Status");
	SmartDashboard::Log(b3, "Ball 3 Status");
	SmartDashboard::Log(flyWheels->isFlyWheelOn(), "Is FlyWheels On?");
}

// Make this return true when this Command no longer needs to run execute()
bool TubeIdle::IsFinished() {
	return false;
}

// Called once after isFinished returns true
void TubeIdle::End() {
	
}

// Called when another command which requires one or more of the same
// subsystems is scheduled to run
void TubeIdle::Interrupted() {
}
